//------------------------------------------------------------------------------
//  <copyright file="FriServer.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------	

#include "stdafx.h"
#include "KukaLBR4Tracing.h"
#include <KukaLBR4Definitions.h>
#include "FriServer.h"
#include "KukaLBR4ControllerCommon.h"

#include <iostream>
#include <KukaLbr4Operations.Manifest.h>

#define DEGREE_PER_RAD (180.0 / M_PI)
#define MM_PER_M 1000

using namespace KukaLBR4ControllerCommonNS;

/// <summary> Initializes a new instance of the <c>FRIServer</c> class. </summary>
FRIServer::FRIServer()
{
	this->pFRIRemote = new friRemote();
	this->Initialize();
}

/// <summary> Initializes a new instance of the <c>FRIServer</c> class.  Uses provided fri remote object. </summary>
/// <param name="pFRIRemoteIn"> The FRI remote object to use. </param>
FRIServer::FRIServer(IFRIRemote *pFRIRemoteIn)
{
	if (pFRIRemoteIn == NULL)
	{
		throw "pFRIRemoteIn cannot be NULL";
	}

	this->pFRIRemote  = pFRIRemoteIn;
	this->Initialize();
}

/// <summary> Initialize state </summary>
void FRIServer::Initialize()
{
	for(int i = 0; i < NUM_JOINTS; ++i)
	{
		this->lastJointCommandSent[i] = 0;
	}
}

/// <summary> dtor. </summary>
FRIServer::~FRIServer()
{
	this->CloseFRI();
}

/// <summary> Close the FRI server. </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIServer::CloseFRI()
{
	delete this->pFRIRemote;
	this->pFRIRemote = NULL;

	return 0;
}

/// <summary> Blocking wait on connection by FRI client </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIServer::WaitOnFRIConnect()
{
	if(this->pFRIRemote == NULL)
	{
		return -1;
	}

	this->pFRIRemote->doReceiveData();

	return 0;
}

/// <summary> Blocking wait on request from FRI client (all control actions are clocked by client). </summary>
/// <param name="registers"> Array of pointers to registers to populate. </param>
/// <returns> Status code.  0 on success. </returns>
int FRIServer::GetFRIRequest(double **ppFeedbackRegisters)
{
	if(this->pFRIRemote == NULL)
	{
		return -1;
	}

	this->pFRIRemote->doReceiveData();

	int i;
	tFriMsrData msr = this->pFRIRemote->getMsrBuf();

	// can't memcpy here because we need to cast from float to double
	for (i = 0; i<CARTESIAN_DIM; ++i)
	{
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::EstimatedTCPForceTorqueRegister][i] = (double)msr.data.estExtTcpFT[i];
	}

	for (i = 0; i<NUM_JOINTS; ++i)
	{
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::CommandedJointOffsetRegister][i] = (double)msr.data.cmdJntPosFriOffset[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::CommandedJointPositionRegister][i] = (double)msr.data.cmdJntPos[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::JointTemperatureRegister][i] = (double)msr.robot.temperature[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::MeasuredExternalJointTorqueRegister][i] = (double)msr.data.estExtJntTrq[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::MeasuredJointPositionRegister][i] = (double)msr.data.msrJntPos[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::MeasuredJointTorqueRegister][i] = (double)msr.data.msrJntTrq[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::LastIssuedJointPositionCommandRegister][i] = this->lastJointCommandSent[i];
	}

	for (i = 0; i<CARTESIAN_FRAME_LENGTH; ++i)
	{
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::CommandedCartesianOffsetRegister][i] = (double)msr.data.cmdCartPosFriOffset[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::CommandedCartesianPositionRegister][i] = (double)msr.data.cmdCartPos[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::MeasuredCartesianPositionRegister][i] = (double)msr.data.msrCartPos[i];
	}

	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::ControllerStrategyRegister]) = msr.robot.control;
	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::CycleTimeInMsRegister]) = msr.intf.desiredMsrSampleTime * 1000.0;
	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::DrivePowerBitFieldRegister]) = msr.robot.power;
	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::FRIStateRegister]) = msr.intf.state;

	for(int i = 0; i<NUM_JOINTS*CARTESIAN_DIM; ++i)
	{
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::JacobianRegister][i] = (double)msr.data.jacobian[i];
		ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::MassMatrixRegister][i] = (double)msr.data.massMatrix[i];
	}

	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::SafetyLimitInPercentRegister]) = (double)msr.intf.safetyLimits;
	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::TimestampInMsRegister]) = (double)msr.intf.timestamp*1000.0;
	*(ppFeedbackRegisters[KukaLBR4PlusFeedbackRegisters::UDPQualityRegister]) = (double)msr.intf.quality;

	return 0;
}

/// <summary> Respond to client but take no control action. </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIServer::DoNothing()
{
	if(this->pFRIRemote == NULL)
	{
		return -1;
	}

	float *currentPositions = this->pFRIRemote->getMsrMsrJntPosition();

	this->pFRIRemote->doPositionControl(currentPositions, false);

	for(int i = 0; i < NUM_JOINTS; ++i)
	{
		this->lastJointCommandSent[i] = currentPositions[i];
	}

	return this->pFRIRemote->doSendData();
}

FRIQuality FRIServer::GetFRIQuality()
{
	if(this->pFRIRemote == NULL)
	{
		return FRIQuality::FRIQualityInvalid;
	}

	switch (this->pFRIRemote->getQuality())
	{
	case FRI_QUALITY_PERFECT:
		return FRIQuality::FRIQualityPerfect;
		break;
	case FRI_QUALITY_OK:
		return FRIQuality::FRIQualityOK;
		break;
	case FRI_QUALITY_BAD:
		return FRIQuality::FRIQualityBad;
		break;
	case FRI_QUALITY_UNACCEPTABLE:
		return FRIQuality::FRIQualityUnaceptable;
		break;
	case FRI_QUALITY_INVALID:
		return FRIQuality::FRIQualityInvalid;
		break;
	}

	return FRIQuality::FRIQualityInvalid;
}

/// <summary> Execute joint position control. </summary>
/// <param name="pJointPosition"> Array of joint positions in order from proximal to distal. </param>
/// <returns> Status code.  0 on no error. </returns>
int FRIServer::DoPositionControl(double *pJointPosition)
{
	if(this->pFRIRemote == NULL)
	{
		return FRI_STATE_INVALID;
	}

	FRI_STATE friState = this->pFRIRemote->getState();

	float pJointPositionFloat[NUM_JOINTS];

	for(int i = 0; i<NUM_JOINTS; i++)
	{
		pJointPositionFloat[i] = (float)pJointPosition[i];
		this->lastJointCommandSent[i] = pJointPosition[i];
	}

	this->pFRIRemote->doPositionControl(pJointPositionFloat, false);

	if(KukaLBR4Tracing::IsControllerEventEnabled(&FRIPositionControlEvent))
	{
		EVENT_DATA_DESCRIPTOR JointPositionDataDescriptor[1];

		EventDataDescCreate(&JointPositionDataDescriptor[0], pJointPositionFloat, 12*sizeof(float));

		KukaLBR4Tracing::WriteKukaLBR4ControllerEvent(&FRIPositionControlEvent,
			_ARRAYSIZE(JointPositionDataDescriptor),
			JointPositionDataDescriptor);
	}

	return this->pFRIRemote->doSendData();
}

/// <summary> Execute joint impedance control. </summary>
/// <param name="pJointPosition"> Array of joint positions in order from proximal to distal. </param>
/// <param name="pJointStiffness"> Array of joint stiffness values in order from proximal to distal. </param>
/// <param name="pJointDamping"> Array of joint damping values in order from proximal to distal. </param>
/// <param name="pJointAdditionalTorque"> Array of addition joint torque values in order from proximal to distal. </param>
/// <returns> Status code.  0 on no error. </returns>
int FRIServer::DoJointImpedanceControl(
	double *pJointPosition,
	double *pJointStiffness,
	double *pJointDamping,
	double *pJointAdditionalTorque)
{
	throw "Not implemented";
}

/// <summary> Execute Cartesian impedance control. </summary>
/// <param name="pCartesianPosition"> Array indicating desired cartesian position of TCP (tool control point) relative to base frame. </param>
/// <param name="pCartesianStiffness"> Array of Cartesian stiffness values in frame of TCP (tool control point). </param>
/// <param name="pCartesianDamping"> Array of Cartesian damping values in frame of TCP (tool control point). </param>
/// <param name="pAdditionalTCPForceTorque"> Array of additional TCP force torque values. CURRENTLY NOT IMPL</param>
int FRIServer::DoCartesianImpedanceControl(
	double *pCartesianPosition,
	double *pCartesianStiffness,
	double *pCartesianDamping,
	double *pAdditionalTCPForceTorque)
{
	if(this->pFRIRemote == NULL)
	{
		return FRI_STATE_INVALID;
	}

	FRI_STATE friState = this->pFRIRemote->getState();

	float pCartesianPositionFloat[CARTESIAN_FRAME_LENGTH];

	for (int i = 0; i<CARTESIAN_FRAME_LENGTH; ++i)
	{
		pCartesianPositionFloat[i] = (float) pCartesianPosition[i];
	}

	if( 1 != this->pFRIRemote->doCartesianImpedanceControl(
		pCartesianPositionFloat,
		NULL, 
		NULL, 
		NULL, 
		NULL,
		false))
	{
		throw "Error doing Cartesian impedance control";
	}

	return this->pFRIRemote->doSendData();
}

/// <summary> Execute Cartesian impedance control by passing position goals to KRL script. </summary>
/// <param name="pCartesianPosition"> Array indicating desired cartesian position of TCP (tool control point) relative to base frame. </param>
/// <param name="sequenceNumber"> Used to identify when a new position command is ready to be executed. </param>
int FRIServer::DoKrlCartesianImpedanceControl(double *pCartesianPosition, int sequenceNumber)
{
	const int krlSequenceNumberIndex = 0;
	const int krlStatusIndex = 1;
	const int krlTurnIndex = 2;

	if(this->pFRIRemote == NULL)
	{
		return FRI_STATE_INVALID;
	}

	// Convert frame to position + ZYX Euler angles (X, Y, Z, A, B, C) as used by KRL
	// X
	this->pFRIRemote->setToKRLReal(0, (float)(MM_PER_M * pCartesianPosition[3]));

	// Y
	double y = MM_PER_M * pCartesianPosition[7];
	this->pFRIRemote->setToKRLReal(1, (float)(y));

	// Z
	this->pFRIRemote->setToKRLReal(2, (float)(MM_PER_M * pCartesianPosition[11]));

	// convert from frame to ZYX Euler angles as described in eq. 2.66 of 3rd edition of Introduction to Robotics by Craig
	// Euler angles, in order (Z-Y-X) are denoted A-B-C below.
	double B = atan2(
		-pCartesianPosition[8],
		sqrt(pCartesianPosition[0] * pCartesianPosition[0] + pCartesianPosition[4] * pCartesianPosition[4]));

	double cosB = cos(B);

	if(fabs(cosB - 0 ) > FLT_EPSILON)
	{
		// A
		this->pFRIRemote->setToKRLReal(3, (float)(DEGREE_PER_RAD * atan2(pCartesianPosition[4]/cosB, pCartesianPosition[0]/cosB)));

		// B
		this->pFRIRemote->setToKRLReal(4, (float)(DEGREE_PER_RAD * B));

		// C
		this->pFRIRemote->setToKRLReal(5, (float)(DEGREE_PER_RAD * atan2(pCartesianPosition[9]/cosB, pCartesianPosition[10]/cosB)));
	}
	else
	{
		// Special case when Cos(B) ~= 0 
		// A
		this->pFRIRemote->setToKRLReal(3, (float)0.0);

		// B
		this->pFRIRemote->setToKRLReal(4, (float)(DEGREE_PER_RAD * B));

		// C
		if(B > 0)
		{
			this->pFRIRemote->setToKRLReal(5, (float)(DEGREE_PER_RAD * atan2(pCartesianPosition[1], pCartesianPosition[5])));
		}
		else
		{
			this->pFRIRemote->setToKRLReal(5, (float)(DEGREE_PER_RAD * -atan2(pCartesianPosition[1], pCartesianPosition[5])));
		}
	}

	// Set sequence number
	this->pFRIRemote->setToKRLInt(krlSequenceNumberIndex, sequenceNumber);

	// Determine status and turn
	double w = 0.5 * sqrt(max(0, 1 + pCartesianPosition[0] + pCartesianPosition[5] + pCartesianPosition[10]));

	UINT8 status = 0;
	UINT8 turn = 0;

	// heuristic decision tree for determining KRL "status" and "turn" values
	// for CNC based pick and place task.
	if (y < -150)
	{
		// CNC area
		if (w > 0.38)
		{
			// side hand
			status = 5;
			turn = 52;
		}
		else
		{
			// over hand
			status = 1;
			turn = 36;
		}
	}
	else if ( y < 100)
	{
		// Neutral area
		if (w > 0.40)
		{
			// side hand
			status = 5;
			turn = 53;
			
		}
		else
		{
			// over hand
			status = 1;
			turn = 45;
		}
	}
	else
	{
		// Transfer area
		if(w > 0.22)
		{
			// side hand
			status = 5;
			turn = 61;
		}
		else
		{
			// over hand
			status = 1;
			turn = 45;
		}
	}

	this->pFRIRemote->setToKRLInt(krlStatusIndex, status);
	this->pFRIRemote->setToKRLInt(krlTurnIndex, turn);

	return this->pFRIRemote->doSendData();
}
